#include "pxt.h"

#include "esp_system.h"
#include "esp_log.h"
#include "esp_vfs.h"
#include "nvs.h"
#include "nvs_flash.h"

#include "driver/gpio.h"
#include "driver/ledc.h"
#include "driver/i2s.h"
#include "driver/spi_master.h"

#include "esp_private/system_internal.h"
#include "esp_partition.h"
#include "esp_ota_ops.h"

#include "esp_pthread.h"
#include "esp_random.h"

// ble related
#include "esp_bt.h"
#include "esp_hidh.h"
#include "esp_hid_gap.h"
#include "esp_bt_defs.h"
#include "esp_gap_ble_api.h"
#include "esp_gatts_api.h"
#include "esp_gatt_defs.h"
#include "esp_bt_main.h"
#include "esp_bt_device.h"

#include "pwm_audio.h"

#include "led_strip.h"

#ifdef BOARD
    #define BOARD_STR(x) #x
    #define BOARD_STR_EXPAND(x) BOARD_STR(x)
    #define BOARD_NAME BOARD_STR_EXPAND(BOARD)
    #pragma message "Compiling for board: " BOARD_NAME
#else
    #error "BOARD not defined"
#endif

#if BOARD == FUTURES3

#define PIN_NEOPIXEL 3
#define PIN_NUM_MISO 47
#define PIN_NUM_MOSI 48
#define PIN_NUM_CLK 45
#define PIN_NUM_DC   44
#define PIN_NUM_CS   46
#define PIN_NUM_BACKLIGHT -1
#define PIN_NUM_RST  -1

#define PIN_NUM_BTNA 0
#define PIN_NUM_BTNB 16

#define BUTTON_SZ 2

#define MIC_CLK 39
#define MIC_DATA 41
#define PDM_OUT 8 // BUZZ
// #define PDM_OUT 5

#elif BOARD == FUTUREBOX

#define PIN_NEOPIXEL 3
#define PIN_NUM_MISO -1
#define PIN_NUM_MOSI 48
#define PIN_NUM_CLK 45
#define PIN_NUM_DC   12
#define PIN_NUM_CS   46
#define PIN_NUM_BACKLIGHT 10
#define PIN_NUM_RST  -1

#define PIN_NUM_BTNA 15
#define PIN_NUM_BTNM 16
#define PIN_NUM_BTNB 42

#define MIC_CLK 39
#define MIC_DATA 41
#define PDM_OUT 8 // BUZZ

#define BUTTON_SZ 3

#else
    #error "Unsupported board"
#endif

extern "C" {

#define INTERNAL_KEY_DOWN 2051
#define INTERNAL_KEY_UP 2050

uint16_t framebuff[160*128];
static uint8_t cmdBuf[20];
static SemaphoreHandle_t xSemaAudio = NULL;
static int pin_led = -1;
static led_strip_handle_t led_strip_buildin = NULL;
static led_strip_handle_t led_strip_ext = NULL;

static bool ble_hid_connected = false;
static uint32_t ble_key_status = 0;

extern int pxt_get_audio_samples(int16_t *buf, unsigned numSamples);
extern void pxt_audio_mute();

enum Key {
    KEY_LEFT = 1,
    KEY_UP = 2,
    KEY_RIGHT = 3,
    KEY_DOWN = 4,
    KEY_A = 5,
    KEY_B = 6,
    KEY_MENU = 7,
};


typedef struct {
    uint8_t cmd;
    uint8_t data[16];
    uint8_t databytes; //No of data in data; bit 7 = delay after set; 0xFF = end of cmds.
} lcd_init_cmd_t;

//Place data into DRAM. Constant data gets placed into DROM by default, which is not accessible by DMA.
DRAM_ATTR static const lcd_init_cmd_t st_init_cmds[]={
    {0x1, {0}, 0x80},
    {0x36, {0x40}, 1},
    /* Interface Pixel Format, 16bits/pixel for RGB/MCU interface */
    {0x3A, {0x55}, 1},
    /* Porch Setting */
    {0xB2, {0x0c, 0x0c, 0x00, 0x33, 0x33}, 5},
    /* Gate Control, Vgh=13.65V, Vgl=-10.43V */
    {0xB7, {0x45}, 1},
    /* VCOM Setting, VCOM=1.175V */
    {0xBB, {0x2B}, 1},
    /* LCM Control, XOR: BGR, MX, MH */
    {0xC0, {0x2C}, 1},
    /* VDV and VRH Command Enable, enable=1 */
    {0xC2, {0x01, 0xff}, 2},
    /* VRH Set, Vap=4.4+... */
    {0xC3, {0x11}, 1},
    /* VDV Set, VDV=0 */
    {0xC4, {0x20}, 1},
    /* Frame Rate Control, 60Hz, inversion=0 */
    {0xC6, {0x0f}, 1},
    /* Power Control 1, AVDD=6.8V, AVCL=-4.8V, VDDS=2.3V */
    {0xD0, {0xA4, 0xA1}, 1},
    /* Positive Voltage Gamma Control */
    {0xE0, {0xD0, 0x00, 0x05, 0x0E, 0x15, 0x0D, 0x37, 0x43, 0x47, 0x09, 0x15, 0x12, 0x16, 0x19}, 14},
    /* Negative Voltage Gamma Control */
    {0xE1, {0xD0, 0x00, 0x05, 0x0D, 0x0C, 0x06, 0x2D, 0x44, 0x40, 0x0E, 0x1C, 0x18, 0x16, 0x19}, 14},
    // invert on
    {0x21, {0}, 0x00},
    // {0x20, {0}, 0x00},
    /* Sleep Out */
    {0x11, {0}, 0x80},
    /* Display On */
    {0x29, {0}, 0x80},
    // addr window
    {0x2A, {0x0,0x0,0,127}, 4},
    {0x2B, {0x0,0x0,0,159}, 4},
    {0, {0}, 0xff}
};

DRAM_ATTR static const lcd_init_cmd_t st_init_cmds2[] = {
    {0x1, {0}, 0x80},
    {0x11, {0}, 0x80},
    {0x20, {0}, 0x80},
    // {0x21, {0}, 0x80},
    {0xB4, {0x01}, 1},
    {0x3A, {0x55}, 1},
    {0x36, {0x60}, 1},
    {0xB1, {0x0,0x06,0x03}, 3},

    {0x2b, {0,0,0,127}, 4},
    {0x2a, {0,0,0,159}, 4},

    {0xE0, {0x02, 0x1c, 0x07, 0x12, 0x37, 0x32, 0x29, 0x2d, 0x29, 0x25, 0x2B, 0x39, 0x00, 0x01, 0x03, 0x10,}, 16},
    {0xE1, {0x03, 0x1d, 0x07, 0x06, 0x2E, 0x2C, 0x29, 0x2D, 0x2E, 0x2E, 0x37, 0x3F, 0x00, 0x00, 0x02, 0x10,}, 16},
    {0x13, {0}, 0x80},
    {0x29, {0}, 0x80},
    {0, {0}, 0xff}
};


const uint8_t font8[] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x5e, 0x00, 0x00, 0x00, 
0x00, 0x0e, 0x00, 0x0e, 0x00, 0x00, 
0x28, 0xfe, 0x28, 0xfe, 0x28, 0x00, 
0x4c, 0x92, 0xff, 0x92, 0x64, 0x00, 
0x02, 0x65, 0x12, 0x48, 0xa6, 0x40, 
0x6c, 0x92, 0x92, 0x6c, 0xa0, 0x00, 
0x00, 0x00, 0x0e, 0x00, 0x00, 0x00, 
0x00, 0x00, 0x7c, 0x82, 0x00, 0x00, 
0x00, 0x00, 0x82, 0x7c, 0x00, 0x00, 
0x54, 0x38, 0x10, 0x38, 0x54, 0x00, 
0x10, 0x10, 0x7c, 0x10, 0x10, 0x00, 
0x00, 0x00, 0x90, 0x70, 0x00, 0x00, 
0x10, 0x10, 0x10, 0x10, 0x10, 0x00, 
0x00, 0x00, 0x60, 0x60, 0x00, 0x00, 
0x00, 0x60, 0x10, 0x08, 0x06, 0x00, 
0x00, 0x3c, 0x42, 0x42, 0x3c, 0x00, 
0x00, 0x44, 0x7e, 0x40, 0x00, 0x00, 
0x00, 0x44, 0x62, 0x52, 0x4c, 0x00, 
0x00, 0x42, 0x4a, 0x4e, 0x32, 0x00, 
0x30, 0x28, 0x24, 0x7e, 0x20, 0x00, 
0x00, 0x4e, 0x4a, 0x4a, 0x32, 0x00, 
0x00, 0x3c, 0x4a, 0x4a, 0x30, 0x00, 
0x00, 0x02, 0x62, 0x12, 0x0e, 0x00, 
0x00, 0x34, 0x4a, 0x4a, 0x34, 0x00, 
0x00, 0x0c, 0x52, 0x52, 0x3c, 0x00, 
0x00, 0x00, 0x6c, 0x6c, 0x00, 0x00, 
0x00, 0x00, 0x96, 0x76, 0x00, 0x00, 
0x10, 0x28, 0x28, 0x44, 0x44, 0x00, 
0x28, 0x28, 0x28, 0x28, 0x28, 0x00, 
0x44, 0x44, 0x28, 0x28, 0x10, 0x00, 
0x00, 0x02, 0x59, 0x09, 0x06, 0x00, 
0x3c, 0x42, 0x5a, 0x56, 0x08, 0x00, 
0x78, 0x14, 0x12, 0x14, 0x78, 0x00, 
0x7e, 0x4a, 0x4a, 0x4a, 0x34, 0x00, 
0x00, 0x3c, 0x42, 0x42, 0x24, 0x00, 
0x00, 0x7e, 0x42, 0x42, 0x3c, 0x00, 
0x00, 0x7e, 0x4a, 0x4a, 0x42, 0x00, 
0x00, 0x7e, 0x0a, 0x0a, 0x02, 0x00, 
0x00, 0x3c, 0x42, 0x52, 0x34, 0x00, 
0x00, 0x7e, 0x08, 0x08, 0x7e, 0x00, 
0x00, 0x42, 0x7e, 0x42, 0x00, 0x00, 
0x20, 0x40, 0x42, 0x3e, 0x02, 0x00, 
0x00, 0x7e, 0x08, 0x14, 0x62, 0x00, 
0x00, 0x7e, 0x40, 0x40, 0x40, 0x00, 
0x7e, 0x04, 0x18, 0x04, 0x7e, 0x00, 
0x00, 0x7e, 0x04, 0x08, 0x7e, 0x00, 
0x3c, 0x42, 0x42, 0x42, 0x3c, 0x00, 
0x00, 0x7e, 0x12, 0x12, 0x0c, 0x00, 
0x00, 0x3c, 0x52, 0x62, 0xbc, 0x00, 
0x00, 0x7e, 0x12, 0x12, 0x6c, 0x00, 
0x00, 0x24, 0x4a, 0x52, 0x24, 0x00, 
0x02, 0x02, 0x7e, 0x02, 0x02, 0x00, 
0x00, 0x3e, 0x40, 0x40, 0x3e, 0x00, 
0x00, 0x1e, 0x70, 0x70, 0x1e, 0x00, 
0x7e, 0x20, 0x18, 0x20, 0x7e, 0x00, 
0x42, 0x24, 0x18, 0x24, 0x42, 0x00, 
0x06, 0x08, 0x70, 0x08, 0x06, 0x00, 
0x00, 0x62, 0x52, 0x4a, 0x46, 0x00, 
0x00, 0x7e, 0x42, 0x42, 0x00, 0x00, 
0x00, 0x06, 0x08, 0x10, 0x60, 0x00, 
0x00, 0x42, 0x42, 0x7e, 0x00, 0x00, 
0x08, 0x04, 0x02, 0x04, 0x08, 0x00, 
0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 
0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 
0x00, 0x30, 0x48, 0x48, 0x78, 0x00, 
0x00, 0x7e, 0x48, 0x48, 0x30, 0x00, 
0x00, 0x30, 0x48, 0x48, 0x48, 0x00, 
0x00, 0x30, 0x48, 0x48, 0x7e, 0x00, 
0x00, 0x30, 0x68, 0x58, 0x50, 0x00, 
0x00, 0x10, 0x7c, 0x12, 0x04, 0x00, 
0x00, 0x18, 0xa4, 0xa4, 0x78, 0x00, 
0x00, 0x7e, 0x08, 0x08, 0x70, 0x00, 
0x00, 0x48, 0x7a, 0x40, 0x00, 0x00, 
0x00, 0x40, 0x84, 0x7d, 0x00, 0x00, 
0x00, 0x7e, 0x10, 0x28, 0x40, 0x00, 
0x00, 0x42, 0x7e, 0x40, 0x00, 0x00, 
0x78, 0x08, 0x30, 0x08, 0x70, 0x00, 
0x00, 0x78, 0x08, 0x08, 0x70, 0x00, 
0x00, 0x30, 0x48, 0x48, 0x30, 0x00, 
0x00, 0xfc, 0x24, 0x24, 0x18, 0x00, 
0x00, 0x18, 0x24, 0x24, 0xfc, 0x00, 
0x00, 0x78, 0x10, 0x08, 0x10, 0x00, 
0x00, 0x50, 0x58, 0x68, 0x28, 0x00, 
0x00, 0x08, 0x3e, 0x48, 0x20, 0x00, 
0x00, 0x38, 0x40, 0x40, 0x78, 0x00, 
0x00, 0x18, 0x60, 0x60, 0x18, 0x00, 
0x38, 0x40, 0x30, 0x40, 0x38, 0x00, 
0x00, 0x48, 0x30, 0x30, 0x48, 0x00, 
0x00, 0x5c, 0xa0, 0xa0, 0x7c, 0x00, 
0x00, 0x48, 0x68, 0x58, 0x48, 0x00, 
0x00, 0x08, 0x36, 0x41, 0x00, 0x00, 
0x00, 0x00, 0xfe, 0x00, 0x00, 0x00, 
0x00, 0x41, 0x36, 0x08, 0x00, 0x00, 
0x00, 0x08, 0x04, 0x08, 0x04, 0x00, 
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
};


static void printch(int x, int y, int color, const uint8_t *fnt) {
    for (int i = 0; i < 6; ++i) {
        uint8_t column = fnt[i];
        for (int j = 0; j < 8; ++j) {
            uint16_t *p = framebuff + (y + j) * 160 + (x + i);
            if (column & (1 << j)) {
                *p = color;
            }
        }
    }
}

static void printTxt(int x, int y, int col, const char *text) {
    int x0 = x;
    while (*text) {
        char c = *text++;
        if (c == '\r')
            continue;
        if (c == '\n') {
            x = x0;
            y += 10;
            continue;
        }
        if (c < ' ' || c >= 0x7f)
            c = '?';
        c -= ' ';
        printch(x, y, col, &font8[c * 6]);
        x += 6;
    }
}

#ifdef MIXER_BUZZ

// TODO: create pwm from note instructions??
void pull_buzzer(){
    

}
#endif

// i2s related
int i2s_common_init(){
    esp_err_t ret;

    i2s_config_t i2s_config = {
        // MODE_PDM will output clock on ws
        .mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_TX | I2S_MODE_PDM),
        .sample_rate = 16000,
        .bits_per_sample = (i2s_bits_per_sample_t)16,
        .channel_format = I2S_CHANNEL_FMT_ONLY_LEFT,
        .communication_format = I2S_COMM_FORMAT_STAND_MSB,
        .dma_buf_count = 3,
        .dma_buf_len = 300,
        .use_apll = true,
        .tx_desc_auto_clear = true,
        .fixed_mclk = 0  
    };
    i2s_driver_uninstall(I2S_NUM_0);
    ret = i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL);
    printf("i2s_driver_install %d\n", ret);
    ret = i2s_set_clk(I2S_NUM_0, 16000, 16, I2S_CHANNEL_MONO);
    printf("i2s_set_clk %d\n", ret);

    // i2s pin config
    i2s_pin_config_t i2s_pin_config = {
        .mck_io_num = -1,
        .bck_io_num = -1,
        .ws_io_num = MIC_CLK,
        .data_out_num = PDM_OUT,
        .data_in_num = MIC_DATA,
    };
    ret = i2s_set_pin(I2S_NUM_0, &i2s_pin_config);
    printf("i2s_set_pin %d\n", ret);
    return ret;
}


#define BUFFER_SIZE 512
void audio_task(void *arg){
    int16_t buf[BUFFER_SIZE];
    xSemaAudio = xSemaphoreCreateBinary();
#ifdef MIXER_PWM
    pwm_audio_config_t pac;
    pac.duty_resolution = LEDC_TIMER_10_BIT;
    pac.gpio_num_left = PDM_OUT;
    pac.ledc_channel_left = LEDC_CHANNEL_7;
    pac.ledc_timer_sel    = LEDC_TIMER_0;
    pac.gpio_num_right    = -1;
    pac.ringbuf_len       = 1024 * 8;
    pwm_audio_init(&pac);
    pwm_audio_set_param(16000, (ledc_timer_bit_t)16, 1);
    pwm_audio_set_volume(16);
#else
    i2s_common_init();
#endif
    int silence = 0;
    while(1){
        if(xSemaphoreTake(xSemaAudio, portMAX_DELAY) == pdTRUE){
            silence = 0;
#ifdef MIXER_PWM
            pwm_audio_start();
#endif
            // i2s_set_clk(I2S_NUM_0, 16000*6, 16, I2S_CHANNEL_MONO); // trick to use same i2s for both audio and mic
            while(1){
                pxt_get_audio_samples(buf, BUFFER_SIZE);
                if (buf[0] == 0){
                    silence++;
                    if (silence > 3){
                        break;
                    }
                }
                size_t bytes_written;
                
#ifdef MIXER_PWM
                pwm_audio_write((uint8_t *)buf, BUFFER_SIZE * 2, &bytes_written, portMAX_DELAY);
#else
                i2s_write(I2S_NUM_0, buf, BUFFER_SIZE * 2, &bytes_written, portMAX_DELAY);
#endif
            }
            // i2s_set_clk(I2S_NUM_0, 16000, 16, I2S_CHANNEL_MONO);
            pxt_audio_mute();
#ifdef MIXER_PWM
            pwm_audio_stop();
#endif
        }
    }
}

void pull_audio(){
    xSemaphoreGive(xSemaAudio);
}

/* Send a command to the LCD. Uses spi_device_polling_transmit, which waits
 * until the transfer is complete.
 *
 * Since command transactions are usually small, they are handled in polling
 * mode for higher speed. The overhead of interrupt transactions is more than
 * just waiting for the transaction to complete.
 */
void lcd_cmd(spi_device_handle_t spi, const uint8_t cmd)
{
    esp_err_t ret;
    spi_transaction_t t;
    memset(&t, 0, sizeof(t));       //Zero out the transaction
    t.length=8;                     //Command is 8 bits
    t.tx_buffer=&cmd;               //The data is the cmd itself
    t.user=(void*)0;                //D/C needs to be set to 0
    ret=spi_device_polling_transmit(spi, &t);  //Transmit!
    assert(ret==ESP_OK);            //Should have had no issues.
}

/* Send data to the LCD. Uses spi_device_polling_transmit, which waits until the
 * transfer is complete.
 *
 * Since data transactions are usually small, they are handled in polling
 * mode for higher speed. The overhead of interrupt transactions is more than
 * just waiting for the transaction to complete.
 */
void lcd_data(spi_device_handle_t spi, const uint8_t *data, int len)
{
    esp_err_t ret;
    spi_transaction_t t;
    if (len==0) return;             //no need to send anything
    memset(&t, 0, sizeof(t));       //Zero out the transaction
    t.length=len*8;                 //Len is in bytes, transaction length is in bits.
    t.tx_buffer=data;               //Data
    t.user=(void*)1;                //D/C needs to be set to 1
    ret=spi_device_polling_transmit(spi, &t);  //Transmit!
    assert(ret==ESP_OK);            //Should have had no issues.
}


uint32_t lcd_get_id(spi_device_handle_t spi)
{
    //get_id cmd
    DMESG("lcd_get_id\n");
    lcd_cmd(spi, 0x04);
    
    DMESG("lcd_get_id #2\n");
    spi_transaction_t t;
    memset(&t, 0, sizeof(t));
    t.length=8*3;
    t.flags = SPI_TRANS_USE_RXDATA;
    t.user = (void*)1;

    esp_err_t ret = spi_device_polling_transmit(spi, &t);
    assert( ret == ESP_OK );

    return *(uint32_t*)t.rx_data;
}

//This function is called (in irq context!) just before a transmission starts. It will
//set the D/C line to the value indicated in the user field.
void lcd_spi_pre_transfer_callback(spi_transaction_t *t)
{
    int dc=(int)t->user;
    gpio_set_level((gpio_num_t)PIN_NUM_DC, dc);
}

static spi_device_handle_t spi;

void pxt_screen_get_pixels(int width, int height, uint16_t *screen);

uint32_t button_pool(){
    uint32_t buttons = 0;
    if (gpio_get_level((gpio_num_t)PIN_NUM_BTNA) == 0)
        buttons |= 1 << KEY_A;
    if (gpio_get_level((gpio_num_t)PIN_NUM_BTNB) == 0)
        buttons |= 1 << KEY_B;
#ifdef PIN_NUM_BTNM
    if (gpio_get_level((gpio_num_t)PIN_NUM_BTNM) == 0)
        buttons |= 1 << KEY_MENU;
#endif
    return buttons;
}

void button_init(){
    gpio_config_t cfg =
    {
        .pin_bit_mask = 0,
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };

// buttons
#ifdef PIN_NUM_BTNA
    cfg.pin_bit_mask = (1ull << PIN_NUM_BTNA);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_BTNA, GPIO_MODE_INPUT);
    gpio_set_pull_mode((gpio_num_t)PIN_NUM_BTNA, GPIO_PULLUP_ONLY);
#endif
#ifdef PIN_NUM_BTNB
    cfg.pin_bit_mask = (1ull << PIN_NUM_BTNB);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_BTNB, GPIO_MODE_INPUT);
    gpio_set_pull_mode((gpio_num_t)PIN_NUM_BTNB, GPIO_PULLUP_ONLY);
#endif
#ifdef PIN_NUM_BTNM
    cfg.pin_bit_mask = (1ull << PIN_NUM_BTNM);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_BTNM, GPIO_MODE_INPUT);
    gpio_set_pull_mode((gpio_num_t)PIN_NUM_BTNM, GPIO_PULLUP_ONLY);
#endif
}

void screen_init(){
    gpio_config_t cfg =
    {
        .pin_bit_mask = 0,
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };

    spi_bus_config_t buscfg={
        .mosi_io_num=PIN_NUM_MOSI,
        .miso_io_num=PIN_NUM_MISO,
        .sclk_io_num=PIN_NUM_CLK,
        .quadwp_io_num=-1,
        .quadhd_io_num=-1,
        // .max_transfer_sz=320
    };
    
    spi_device_interface_config_t devcfg = {
        .mode = 0,
        .clock_speed_hz = 10 * 1000 * 1000,
        .spics_io_num = PIN_NUM_CS,
        .queue_size = 7,
        .pre_cb=lcd_spi_pre_transfer_callback
    };

    cfg.pin_bit_mask = (1ull << PIN_NUM_DC);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_DC, GPIO_MODE_OUTPUT);

#if PIN_NUM_RST != -1
    cfg.pin_bit_mask = (1ull << PIN_NUM_RST);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_RST, GPIO_MODE_OUTPUT);
    gpio_set_level((gpio_num_t)PIN_NUM_RST, 0);
    vTaskDelay(pdMS_TO_TICKS(100));
    gpio_set_level((gpio_num_t)PIN_NUM_RST, 1);
    vTaskDelay(pdMS_TO_TICKS(100));
#endif

#if PIN_NUM_BACKLIGHT != -1
    cfg.pin_bit_mask = (1ull << PIN_NUM_BACKLIGHT);
    gpio_config(&cfg);
    gpio_set_direction((gpio_num_t)PIN_NUM_BACKLIGHT, GPIO_MODE_OUTPUT);
    gpio_set_level((gpio_num_t)PIN_NUM_BACKLIGHT, 1);
#endif

    int ret = spi_bus_initialize(SPI2_HOST, &buscfg, SPI_DMA_CH_AUTO);
    DMESG("spi init %d", ret);
    ret = spi_bus_add_device(SPI2_HOST, &devcfg, &spi);
    DMESG("spi init2 %d", ret);

    uint32_t lcd_id = lcd_get_id(spi);
    DMESG("lcd_id %d", lcd_id);
}

void screen_addr_window(int x, int y, int w, int h){
    w += x - 1;
    h += y - 1;
    uint8_t cmd0[] = {0, (uint8_t)x, (uint8_t)(w >> 8), (uint8_t)w};
    uint8_t cmd1[] = {0, (uint8_t)y, (uint8_t)(h >> 8), (uint8_t)h};
    lcd_cmd(spi, 0x2a);
    lcd_data(spi, cmd0, sizeof(cmd0));
    lcd_cmd(spi, 0x2b);
    lcd_data(spi, cmd1, sizeof(cmd1));
}

void screen_update(){
     lcd_cmd(spi, 0x2c);
    for (int line = 0; line < 128; line++) {
        lcd_data(spi, (uint8_t*)framebuff + 160 * line * 2, 160 * 2);
    }
}

void screen_status_print(const char * str){
    int len = strlen(str);
    screen_addr_window(0, 120, 160, 8);
    // fill status bar with black
    for (int i=0;i<160*8;i++)
        framebuff[i] = 0;
        
    // print font8 to screen
    printTxt(0, 0, 0xffff, str);
}

void screen_panic(const char * str, uint16_t background){
    memset(framebuff, background, 160*128*2);
    printTxt(10, 20, 0xffff, str);
}

void screen_task(void *pvParameter){
    //Send all the commands
    int cmd=0;
    uint32_t state = 0;
#if BOARD == FUTURES3
    DMESG("Board: FUTURES3\n");
    const lcd_init_cmd_t* lcd_init_cmds = st_init_cmds2;
#elif BOARD == FUTUREBOX
    DMESG("Board: FUTUREBOX\n");
    const lcd_init_cmd_t* lcd_init_cmds = st_init_cmds2;
#endif

    while (lcd_init_cmds[cmd].databytes!=0xff) {
        lcd_cmd(spi, lcd_init_cmds[cmd].cmd);
        lcd_data(spi, lcd_init_cmds[cmd].data, lcd_init_cmds[cmd].databytes&0x1F);
        if (lcd_init_cmds[cmd].databytes&0x80) {
            vTaskDelay(pdMS_TO_TICKS(100));
        }
        cmd++;
    }
    screen_addr_window(0, 0, 160, 128);

    // uint8_t rgb[2] = {0xf0,0x00};
    // lcd_cmd(spi, 0x2c);
    // for (int i=0;i<160*128;i++)
    //     lcd_data(spi,rgb,2);
    // vTaskDelay(pdMS_TO_TICKS(1000));
    while (true){
        // printf("t=%lld\n", esp_timer_get_time() / 1000);
        uint32_t nstate = 0;
        if (ble_hid_connected){
            nstate = ble_key_status;
        } else {
            nstate = button_pool();
        }
        if (nstate != state){
            // ESP_LOGI("button", "state %x", nstate);
            for (int i=0;i<8;i++){
                uint32_t mask = 1 << i;
                int ev = 0;
                if ((state & mask) && !(nstate & mask))
                    ev = INTERNAL_KEY_UP;
                else if (!(state & mask) && (nstate & mask)) {
                    ev = INTERNAL_KEY_DOWN;
                }
                if (ev){
                    raiseEvent(ev, i);
                    raiseEvent(ev, 0);
                }
            }
            state = nstate;
        }

        // skip status bar 
        if (panicCode == 0)
            // pxt_screen_get_pixels(160, 120, &framebuff[160*8]);
            pxt_screen_get_pixels(160, 120, &framebuff[0]);
        // render to screen
        screen_update();
        vTaskDelay(pdMS_TO_TICKS(5)); // prevent wdt
    }
}

void led_init(int led, uint32_t len, led_strip_handle_t * led_strip){
    led_strip_config_t strip_config = {
        .strip_gpio_num = led,   // The GPIO that connected to the LED strip's data line
        .max_leds = len,        // The number of LEDs in the strip,
        .led_pixel_format = LED_PIXEL_FORMAT_GRB, // Pixel format of your LED strip
        .led_model = LED_MODEL_WS2812,            // LED strip model
        .flags = {
            .invert_out = false
        }
    };
    // use rmt
    led_strip_rmt_config_t rmt_config = {
        .clk_src = RMT_CLK_SRC_DEFAULT,        // different clock source can lead to different power consumption
        .resolution_hz = (10 * 1000 * 1000),   // RMT counter clock frequency
        .flags{
            .with_dma = false,               // DMA feature is available on ESP target like ESP32-S3
        }
    };
    led_strip_new_rmt_device(&strip_config, &rmt_config, led_strip);
}

char * load_default_program(){
    if (gpio_get_level((gpio_num_t)PIN_NUM_BTNB) == 0){
        return NULL;
    }
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
        printf("nvs_flash_init %d\n", ret);
        return NULL;
    }
    nvs_handle_t nvs;
    ret = nvs_open("storage", NVS_READWRITE, &nvs);
    if (ret != ESP_OK) {
        return NULL;
    }
    size_t size = 0;
    ret = nvs_get_blob(nvs, "default", NULL, &size);
    if (ret != ESP_OK) {
        nvs_close(nvs);
        return NULL;
    }
    char *data = (char *)malloc(64);
    memset(data, 0, 64);
    ret = nvs_get_blob(nvs, "default", data, &size);
    if (ret != ESP_OK) {
        nvs_close(nvs);
        free(data);
        return NULL;
    }
    nvs_close(nvs);
    return data;
}

void save_default_program(const char *data, uint32_t len){
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
        printf("nvs_flash_init %d\n", ret);
        return;
    }
    nvs_handle_t nvs;
    ret = nvs_open("storage", NVS_READWRITE, &nvs);
    if (ret != ESP_OK) {
        printf("nvs_open fail %d\n", ret);
        return;
    }
    ret = nvs_set_blob(nvs, "default", data, len);
    if (ret != ESP_OK) {
        printf("nvs_set_blob fail %d\n", ret);
        nvs_close(nvs);
        return;
    }
    nvs_close(nvs);
}

static char *bda2str(esp_bd_addr_t bda, char *str, size_t size)
{
    if (bda == NULL || str == NULL || size < 18) {
        return NULL;
    }

    uint8_t *p = bda;
    sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
            p[0], p[1], p[2], p[3], p[4], p[5]);
    return str;
}


void hidh_callback(void *handler_args, esp_event_base_t base, int32_t id, void *event_data)
{
    esp_hidh_event_t event = (esp_hidh_event_t)id;
    esp_hidh_event_data_t *param = (esp_hidh_event_data_t *)event_data;

    switch (event) {
    case ESP_HIDH_OPEN_EVENT: {
        if (param->open.status == ESP_OK) {
            const uint8_t *bda = esp_hidh_dev_bda_get(param->open.dev);
            ESP_LOGI("BLEHID", ESP_BD_ADDR_STR " OPEN: %s", ESP_BD_ADDR_HEX(bda), esp_hidh_dev_name_get(param->open.dev));
            esp_hidh_dev_dump(param->open.dev, stdout);
            ble_hid_connected = true;
        } else {
            ESP_LOGE("BLEHID", " OPEN failed!");
        }
        break;
    }
    case ESP_HIDH_BATTERY_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->battery.dev);
        ESP_LOGI("BLEHID", ESP_BD_ADDR_STR " BATTERY: %d%%", ESP_BD_ADDR_HEX(bda), param->battery.level);
        break;
    }
    case ESP_HIDH_INPUT_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->input.dev);
        // ESP_LOGI("BLEHID", ESP_BD_ADDR_STR " INPUT: %8s, MAP: %2u, ID: %3u, Len: %d, Data:", ESP_BD_ADDR_HEX(bda), esp_hid_usage_str(param->input.usage), param->input.map_index, param->input.report_id, param->input.length);
        // ESP_LOG_BUFFER_HEX("BLEHID", param->input.data, param->input.length);
        uint8_t _dir = param->input.data[4];
        uint8_t _btn = param->input.data[5];
        // ESP_LOGI("BLEHID", "dir %x, btn %x", _dir, _btn);
        ble_key_status = 0;
        if (_dir == 0x00){
            ble_key_status |= 1 << KEY_UP;
        } else if (_dir == 0x02){
            ble_key_status |= 1 << KEY_RIGHT;
        } else if (_dir == 0x04){
            ble_key_status |= 1 << KEY_DOWN;
        } else if (_dir == 0x06){
            ble_key_status |= 1 << KEY_LEFT;
        }
        if (_btn == 0x01){
            ble_key_status |= 1 << KEY_A;
        } else if (_btn == 0x02){
            ble_key_status |= 1 << KEY_B;
        }
        break;
    }
    case ESP_HIDH_FEATURE_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->feature.dev);
        ESP_LOGI("BLEHID", ESP_BD_ADDR_STR " FEATURE: %8s, MAP: %2u, ID: %3u, Len: %d", ESP_BD_ADDR_HEX(bda),
                 esp_hid_usage_str(param->feature.usage), param->feature.map_index, param->feature.report_id,
                 param->feature.length);
        ESP_LOG_BUFFER_HEX("BLEHID", param->feature.data, param->feature.length);
        break;
    }
    case ESP_HIDH_CLOSE_EVENT: {
        const uint8_t *bda = esp_hidh_dev_bda_get(param->close.dev);
        ESP_LOGI("BLEHID", ESP_BD_ADDR_STR " CLOSE: %s", ESP_BD_ADDR_HEX(bda), esp_hidh_dev_name_get(param->close.dev));
        ble_hid_connected = false;
        break;
    }
    default:
        ESP_LOGI("BLEHID", "EVENT: %d", event);
        break;
    }
}

#define SCAN_DURATION_SECONDS 5
void ble_hid_task(void *pvParameter){
    size_t results_len = 0;
    esp_hid_scan_result_t *results = NULL;
    ESP_LOGI("BLEHID", "SCAN...");
    //start scan for HID devices
    esp_hid_scan(SCAN_DURATION_SECONDS, &results_len, &results);
    ESP_LOGI("BLEHID", "SCAN: %u results", results_len);
    if (results_len) {
        esp_hid_scan_result_t *r = results;
        esp_hid_scan_result_t *cr = NULL;
        while (r) {
            printf("  %s: " ESP_BD_ADDR_STR ", ", (r->transport == ESP_HID_TRANSPORT_BLE) ? "BLE" : "BT ", ESP_BD_ADDR_HEX(r->bda));
            printf("RSSI: %d, ", r->rssi);
            printf("USAGE: %s, ", esp_hid_usage_str(r->usage));
            if (r->transport == ESP_HID_TRANSPORT_BLE) {
                cr = r;
                printf("APPEARANCE: 0x%04x, ", r->ble.appearance);
                printf("ADDR_TYPE: '%s', ", ble_addr_type_str(r->ble.addr_type));
            }
            printf("NAME: %s ", r->name ? r->name : "");
            printf("\n");
            r = r->next;
        }
        if (cr) {
            //open the last result
            esp_hidh_dev_open(cr->bda, cr->transport, cr->ble.addr_type);
        }
        //free the results
        esp_hid_scan_results_free(results);
    }
    vTaskDelete(NULL);
}

void ble_hid_init(){
    char bda_str[18] = {0};
    ESP_ERROR_CHECK( esp_hid_gap_init(HID_HOST_MODE) );
    ESP_ERROR_CHECK( esp_ble_gattc_register_callback(esp_hidh_gattc_event_handler) );
    esp_hidh_config_t config = {
        .callback = hidh_callback,
        .event_stack_size = 4096,
        .callback_arg = NULL,
    };
    ESP_ERROR_CHECK( esp_hidh_init(&config) );

    ESP_LOGI("BLEHID", "Own address:[%s]", bda2str((uint8_t *)esp_bt_dev_get_address(), bda_str, sizeof(bda_str)));

    xTaskCreate(&ble_hid_task, "hid_task", 6 * 1024, NULL, 2, NULL);
}

} // extern "C"

static void reset_pin(PinName p) {
    if (p != -1)
        gpio_set_direction((gpio_num_t)p, GPIO_MODE_DISABLE);
}

void reboot_to_uf2(void) {
    reset_pin(PIN(LED_R));
    reset_pin(PIN(LED_G));
    reset_pin(PIN(LED_B));

#if defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)
    // call esp_reset_reason() is required for idf.py to properly links esp_reset_reason_set_hint()
    (void)esp_reset_reason();
    esp_reset_reason_set_hint((esp_reset_reason_t)0x11F2);
#endif

    esp_restart();
}

namespace pxt {

static void initRandomSeed() {
    seedRandom(esp_random());
}

extern "C" void drawPanic(int code) {
    // TODO
}

#if CONFIG_IDF_TARGET_ESP32S2
static void bootloader_handler(void *) {
    reboot_to_uf2();
}

void install_gpio0_handler() {
    DMESG("reset reason: %x", esp_reset_reason_get_hint());
    gpio_install_isr_service(0);
    gpio_config_t gpio_conf;
    gpio_conf.intr_type = GPIO_INTR_POSEDGE;
    gpio_conf.mode = GPIO_MODE_INPUT;
    gpio_conf.pin_bit_mask = (1ULL << GPIO_NUM_0);
    gpio_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    gpio_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    gpio_config(&gpio_conf);
    gpio_isr_handler_add(GPIO_NUM_0, bootloader_handler, NULL);
}
#else
void install_gpio0_handler() {
    gpio_install_isr_service(0);
}
#endif

extern "C" void target_init() {
    initRandomSeed();
    memInfo();
    // usb_init();

    screen_init();

    xTaskCreatePinnedToCore(&screen_task, "screen_task", 4096, NULL, 5, NULL, 1);
    xTaskCreatePinnedToCore(&audio_task, "audio_task", 4096, NULL, 6, NULL, 1);

    ble_hid_init();
}

void updateScreen(Image_ img);

void screen_init() {
    updateScreen(NULL);
}

void memInfo() {
    // heap_caps_print_heap_info(MALLOC_CAP_DEFAULT);
    DMESG("mem: %d free (%d total)", heap_caps_get_free_size(MALLOC_CAP_DEFAULT),
          heap_caps_get_total_size(MALLOC_CAP_DEFAULT));
}

void (*logJDFrame)(const uint8_t *data);
void (*sendJDFrame)(const uint8_t *data);

} // namespace pxt


namespace control {

//%
RefCollection *programList() {
    auto res = Array_::mk();
    DMESG("programList run\n");

    // Add sorted files to the result array
    for (int i = 0; i < MAX_FILES; ++i) {
        if (entries[i].name == NULL) {
            break;
        }
        auto tmp = (TValue)mkString(entries[i].name, -1);
        registerGCPtr(tmp);
        res->head.push(tmp);
        unregisterGCPtr(tmp);
        free((void *)entries[i].name);  // 释放 strdup 分配的内存
    }

    return res;
}

//%
void runProgram(String prog) {
    DMESG("runProgram %s\n", PXT_STRING_DATA(prog));
    uint8_t *data;
    int len;
    // data = usb_load_pxt(PXT_STRING_DATA(prog));
    
    save_default_program(PXT_STRING_DATA(prog), PXT_STRING_DATA_LENGTH(prog));

    // restart with current app partition
    const esp_partition_t *running = esp_ota_get_running_partition();
    esp_ota_set_boot_partition(running);

    esp_restart();
}


} // namespace control


namespace light {

//%
void sendBuffer1(int pin, Buffer buf) {
    led_strip_handle_t led_strip;
    if (pin == PIN_NEOPIXEL && !led_strip_buildin) {
        led_strip = led_strip_buildin;
        led_init(PIN_NEOPIXEL, buf->length / 3, &led_strip);
        // ESP_LOGI("light", "buildin led strip");
    } else {
        led_strip = led_strip_ext;
        if (pin_led != pin) {
            led_init(pin, buf->length / 3, &led_strip_ext);
            pin_led = pin;
            led_strip = led_strip_ext;
            // ESP_LOGI("light", "external led strip");
        }
    }
    for (int i=0;i<buf->length;i+=3){
        // TODO: just switch gbr??
        led_strip_set_pixel(led_strip, i/3, buf->data[i+1], buf->data[i+0], buf->data[i+2]);
    }
    led_strip_refresh(led_strip);
}

} // namespace light